Advanced Lane Finding¶
As a part of the Udacity Self Driving Car Engineer Nanodegree program, we use computer vision to implement to again implement lane finding, but using a more advanced set of techniques than the first project of the course.
Advanced Lane Finding Project
The goals / steps of this project are the following:
- Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.
- Apply a distortion correction to raw images.
- Use color transforms, gradients, etc., to create a thresholded binary image.
- Apply a perspective transform to rectify binary image ("birds-eye view").
- Detect lane pixels and fit to find the lane boundary.
- Determine the curvature of the lane and vehicle position with respect to center.
- Warp the detected lane boundaries back onto the original image.
- Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.
In [1]:
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
%matplotlib inline
def getpoints(imgfiles, nx, ny, drawcorners=False, allowoffbyone=True):
'''Construct image points and object points from chessboard calibration image files'''
imgpoints = []
objpoints = []
imgsize = None
# prepare constant object points to append if corners found in an image
objp = np.zeros((nx*ny,3), dtype=np.float32)
objp[:,:2] = np.mgrid[:nx,:ny].T.reshape(-1,2)
# Process images and build imgpoints list
for imgfile in imgfiles:
img = mpimg.imread(imgfile)
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
this_imgsize = gray.shape[::-1]
if imgsize is None:
imgsize = this_imgsize # store (width, height) for use by cv2.calibrateCamera()
else:
msg = 'Size mismatch: {}. {} != {}.'.format(imgfile, imgsize, gray.shape[::-1])
# a couple of the images have an extra pixel in each dimension, but it should be
# ok since the corners are within the smaller dimension range?
# Thus, let's optionally allow it.
allowedsizes = [this_imgsize]
if allowoffbyone:
allowedsizes.append((this_imgsize[0] - 1, this_imgsize[1] - 1))
assert imgsize in allowedsizes, msg
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
# If corners found, draw corners, and append corresponding img/obj points
if ret:
# Optionally plot the images with corners drawn
if drawcorners:
cv2.drawChessboardCorners(img, (nx, ny), corners, ret)
plt.figure()
plt.imshow(img)
plt.title(imgfile + ' corners')
objpoints.append(objp)
imgpoints.append(corners)
return objpoints, imgpoints, imgsize
In [2]:
import glob
import os
# Make a list of calibration image files
imgfiles = glob.glob('camera_cal/calibration*.jpg')
# Get corresponding object/image points assuming 9x6 checkerboard
objpoints, imgpoints, imgsize = getpoints(imgfiles, nx=9, ny=6, drawcorners=True)
# Compute calibration matrix and distortion coefficients
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, imgsize, None, None)
# Load a calibration image and undistort it
examplefile = imgfiles[0]
img = mpimg.imread(examplefile)
dst = cv2.undistort(img, mtx, dist, None, mtx)
# Plot the before and after
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15,30))
ax1.imshow(img)
ax1.set_title(examplefile, fontsize=20)
ax2.imshow(dst)
ax2.set_title(examplefile + ' undistorted', fontsize=20)
name, ext = os.path.splitext(os.path.basename(examplefile))
fig.savefig('examples/' + name + '_undistortion' + ext, bbox_inches='tight')
assert ret, "Problem Calibrating Camera!"
In [3]:
examplefiles = glob.glob('test_images/test*.jpg')
for examplefile in examplefiles:
# Load a test image and undistort it
img = mpimg.imread(examplefile)
dst = cv2.undistort(img, mtx, dist, None, mtx)
# Plot the before and after
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15,30))
ax1.imshow(img)
ax1.set_title(examplefile, fontsize=15)
ax2.imshow(dst)
ax2.set_title(examplefile + ' undistorted', fontsize=15)
name, ext = os.path.splitext(os.path.basename(examplefile))
fig.savefig('examples/' + name + '_undistortion' + ext, bbox_inches='tight')
3. Use color transforms, gradients, etc., to create a thresholded binary image.¶
In [4]:
def get_color_channel(img, cvtmode, channel_idx=0):
'''Apply desired color conversion and return the desired channel'''
# Convert color space
img_cvt = cv2.cvtColor(img, cvtmode)
# Select desired color channel
img_cvt = np.atleast_3d(img_cvt) # in case it's grayscale
channel_img = img_cvt[:, :, channel_idx]
return channel_img
def deriv_mag_thresh(img, orient='x', sobel_kernel=3, thresh=(0, 255),
cvtmode=cv2.COLOR_RGB2GRAY, channel_idx=0):
'''Apply Sobel x or y, take the absolute value and apply thresholds
to produce boolean pixel mask'''
# Convert colorspace and select desired channel
ch_img = get_color_channel(img, cvtmode, channel_idx=channel_idx)
# Take the partial derivative wrt x or y given orient = 'x' or 'y'
if orient=='x':
sobel = cv2.Sobel(ch_img, cv2.CV_64F, 1, 0)
elif orient=='y':
sobel = cv2.Sobel(ch_img, cv2.CV_64F, 0, 1)
else:
raise ValueError("orient can be 'x' or 'y'")
# Take the absolute value of that partial derivative
abs_sobel = np.abs(sobel)
# Scale to 8-bit (0 - 255) then convert to type = np.uint8
scaled_sobel = np.uint8(255 * abs_sobel / np.max(abs_sobel))
# Create a boolean mask of 'True' pixels using thresh interval
boolean_mask = (scaled_sobel > thresh[0]) & (scaled_sobel < thresh[1])
return boolean_mask
def grad_mag_thresh(img, sobel_kernel=3, mag_thresh=(0, 255),
cvtmode=cv2.COLOR_RGB2GRAY, channel_idx=0):
'''Apply Sobel x and y, compute the magnitude of the gradient
and apply thresholds to produce boolean pixel mask'''
# Convert colorspace and select desired channel
ch_img = get_color_channel(img, cvtmode, channel_idx=channel_idx)
# Compute the x and y components of the gradient
sobelx = cv2.Sobel(ch_img, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobely = cv2.Sobel(ch_img, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# Calculate the gradient magnitude
mag = np.sqrt(sobelx**2 + sobely**2)
# Scale to 8-bit (0 - 255) and convert to type = np.uint8
scaled_mag = np.uint8(255 * mag / mag.max())
# Create a boolean mask of 'True' pixels using thresh interval
boolean_mask = (scaled_mag >= mag_thresh[0]) & (scaled_mag <= mag_thresh[1])
return boolean_mask
def grad_dir_thresh(img, sobel_kernel=3, thresh=(0, np.pi/2),
cvtmode=cv2.COLOR_RGB2GRAY, channel_idx=0):
'''Apply Sobel x and y, compute the gradient direction and apply
thresholds to produce boolean pixel mask'''
# Convert colorspace and select desired channel
ch_img = get_color_channel(img, cvtmode, channel_idx=channel_idx)
# Take the gradient in x and y separately
sobelx = cv2.Sobel(ch_img, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobely = cv2.Sobel(ch_img, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# Take the absolute value of the x and y gradients
abs_sobelx = np.abs(sobelx)
abs_sobely = np.abs(sobely)
# Use np.arctan2(abs_sobely, abs_sobelx) to calculate the direction of the gradient
angle = np.arctan2(abs_sobely, abs_sobelx)
# Create a boolean mask of 'True' pixels using thresh interval
boolean_mask = (angle > thresh[0]) & (angle < thresh[1])
return boolean_mask
def color_thresh(img, thresh=(0, 255), cvtmode=cv2.COLOR_RGB2HLS,
channel_idx=2):
'''Optionally convert to another colorspace, then threshold the desired channel
to produce a boolean pixel mask.
By default it takes an RGB image, converts it to HLS and thresholds the
S channel to produce a boolean pixel mask'''
# Convert colorspace and select desired channel
ch_img = get_color_channel(img, cvtmode, channel_idx=channel_idx)
# Create a boolean mask of 'True' pixels using thresh interval
boolean_mask = (ch_img > thresh[0]) & (ch_img <= thresh[1])
return boolean_mask
def get_pixel_mask(img):
'''Use colorspace conversions, color thresholds and gradients to produce images mapped
to max/min pixel values representing a boolean mask of selected pixels. Returns
gradient/color combined as well as stacked for debug/analysis.'''
# Create boolean masks using each technique
x_mask = deriv_mag_thresh(img, orient='x', sobel_kernel=3, thresh=(20, 100),
cvtmode=cv2.COLOR_RGB2HLS, channel_idx=1)
y_mask = deriv_mag_thresh(img, orient='y', sobel_kernel=3, thresh=(20, 100),
cvtmode=cv2.COLOR_RGB2HLS, channel_idx=1)
mag_mask = grad_mag_thresh(img, sobel_kernel=9, mag_thresh=(30, 100),
cvtmode=cv2.COLOR_RGB2HLS, channel_idx=1)
dir_mask = grad_dir_thresh(img, sobel_kernel=15, thresh=(0.7, 1.3),
cvtmode=cv2.COLOR_RGB2HLS, channel_idx=1)
color_mask = color_thresh(img, thresh=(170, 255), cvtmode=cv2.COLOR_RGB2HLS, channel_idx=2)
# Combine the partial derivative and gradient magnitude and direction masks
grad_mask = (x_mask) | (mag_mask & dir_mask)
# Combine gradient and color masks
grad_color_mask = grad_mask | color_mask
# Create black and white image of the fully combined mask
grad_color_img = np.uint8(grad_color_mask*255)
# Create stacked image of color and grad masks for visualizing the contribution of each
grad_color_img_stacked = np.dstack((np.zeros_like(grad_mask, np.uint8),
np.uint8(grad_mask*255),
np.uint8(color_mask*255)))
return grad_color_img, grad_color_img_stacked
In [5]:
examplefiles = glob.glob('test_images/test*.jpg')
for examplefile in examplefiles:
# Load a test image
img = mpimg.imread(examplefile)
# Correct for camera distortion
dst = cv2.undistort(img, mtx, dist, None, mtx)
# Get binary mask images
combined, stacked = get_pixel_mask(dst)
# Plot the before and after
fig, ax = plt.subplots(2, 2, figsize=(16,11))
ax[0,0].imshow(dst)
ax[0,0].set_title(examplefile, fontsize=15)
ax[0,1].imshow(dst)
ax[0,1].set_title(examplefile + ' undistorted', fontsize=15)
ax[1,0].imshow(stacked)
ax[1,0].set_title(examplefile + ' undistorted\nstacked gradient & color masks', fontsize=15)
ax[1,1].imshow(combined, cmap='gray')
ax[1,1].set_title(examplefile + ' undistorted\ncombined gradient & color masks', fontsize=15)
name, ext = os.path.splitext(os.path.basename(examplefile))
fig.savefig('examples/' + name + '_binary' + ext, bbox_inches='tight')
4. Apply a perspective transform to rectify binary image ("birds-eye view").¶
In [6]:
import tabulate
# src points rectangle vertices
srcpoints = np.float32(
[[(imgsize[0] / 2) - 60, imgsize[1] / 2 + 100],
[((imgsize[0] / 6) - 10), imgsize[1]],
[(imgsize[0] * 5 / 6) + 40, imgsize[1]],
[(imgsize[0] / 2 + 64), imgsize[1] / 2 + 100]])
# dst points rectangle vertices
dstpoints = np.float32(
[[(imgsize[0] / 4), 0],
[(imgsize[0] / 4), imgsize[1]],
[(imgsize[0] * 3 / 4), imgsize[1]],
[(imgsize[0] * 3 / 4), 0]])
print(tabulate.tabulate({'srcpoints':srcpoints, 'dstpoints':dstpoints},
headers='keys', tablefmt='pipe'))
# Compute perspective transform matrix, M
M = cv2.getPerspectiveTransform(srcpoints, dstpoints)
# Compute inverse perspective transform matrix, Minv (used later to draw computed lane onto scene)
Minv = cv2.getPerspectiveTransform(dstpoints, srcpoints)
In [7]:
straight_examplefiles = glob.glob('test_images/straight_lines*.jpg')
# Create subplot axes with appropriate shape and size
fig, ax = plt.subplots(2, 2, figsize=(16, 11))
for idx, examplefile in enumerate(straight_examplefiles):
# load image
img = mpimg.imread(examplefile)
# correct for camera distortion
undist = cv2.undistort(img, mtx, dist, None, mtx)
# Warp image perspective to a top-down view
warped = cv2.warpPerspective(undist, M, imgsize, flags=cv2.INTER_LINEAR)
# Draw source and destination point "rectangles"
cv2.polylines(undist,[srcpoints.reshape((-1,1,2)).astype(np.int32)],True,(255,0,0), thickness=4)
cv2.polylines(warped,[dstpoints.reshape((-1,1,2)).astype(np.int32)],True,(255,0,0), thickness=8)
# Plot them
ax[idx,0].imshow(undist)
ax[idx,0].set_title(examplefile, fontsize=15)
ax[idx,1].imshow(warped)
ax[idx,1].set_title(examplefile + ' warped', fontsize=15)
fig.savefig('examples/straight_lines_warped.jpg', bbox_inches='tight')
In [8]:
examplefiles = glob.glob('test_images/*.jpg')
for examplefile in examplefiles:
# Load a test image
img = mpimg.imread(examplefile)
# Correct for camera distortion
dst = cv2.undistort(img, mtx, dist, None, mtx)
# Get binary mask image
combined,_ = get_pixel_mask(dst)
# Warp binary mask images
combined_warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
# Draw source and destination point "rectangles"
combined = np.dstack([combined]*3)
combined_warped = np.dstack([combined_warped]*3)
cv2.polylines(combined,[srcpoints.reshape((-1,1,2)).astype(np.int32)],True,(255,0,0), thickness=4)
cv2.polylines(combined_warped,[dstpoints.reshape((-1,1,2)).astype(np.int32)],True,(255,0,0), thickness=8)
# Plot them
fig, ax = plt.subplots(1, 2, figsize=(16, 5.5))
ax[0].imshow(combined)
ax[0].set_title(examplefile + ' mask', fontsize=15)
ax[1].imshow(combined_warped)
ax[1].set_title(examplefile + ' mask warped', fontsize=15)
name, ext = os.path.splitext(os.path.basename(examplefile))
fig.savefig('examples/' + name + '_binary_warped' + ext, bbox_inches='tight')
5. Detect lane pixels and fit to find the lane boundary.¶
In [9]:
def get_windowed_pts(image, window_center, window_width, window_height, out_img=None, level=0):
'''Get pixel (y,x) points within the window'''
# Identify the x and y positions of all nonzero pixels in the image
nonzero = np.transpose(image.nonzero())
nonzeroy = nonzero[:,0]
nonzerox = nonzero[:,1]
# Identify window boundaries in x and y (and right and left)
win_y_low = np.int(image.shape[0] - (level + 1)*window_height)
win_y_high = np.int(image.shape[0] - level*window_height)
win_x_low = np.int(window_center - window_width//2)
win_x_high = np.int(window_center + window_width//2)
if out_img is not None:
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_x_low,win_y_low),(win_x_high,win_y_high),(0,255,0), 3)
# Identify the nonzero pixels in x and y within the window
windowed_pts = nonzero[(nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
(nonzerox >= win_x_low) & (nonzerox < win_x_high), :]
return windowed_pts
def draw_polyfit(lane_pts, out_img):
'''Draw 2nd degree polynomial fit of lane_pts on out_img if out_img is not None'''
if out_img is None:
return # no image to draw on, do nothing
# Fit a second order polynomial to each
fit = np.polyfit(lane_pts[:, 0], lane_pts[:, 1], 2)
# Generate x and y values for plotting the polynomials
ploty = np.arange(0, out_img.shape[0],10)
fitx = np.polyval(fit, ploty)
# Draw the polynomials
pts = np.stack((np.int32(np.round(fitx)), np.int32(ploty)), axis=1).reshape((-1,1,2))
cv2.polylines(out_img,[pts], False,(255,255,0), 10)
def compute_conv_signal(image, window_width, window_height, level):
# convolve the window into the vertical slice of the image
image_layer = np.sum(image[int(image.shape[0]-(level+1)*window_height):
int(image.shape[0]-level*window_height),:], axis=0)
conv_signal = np.convolve(np.ones(window_width), image_layer)
return conv_signal
def compute_window_center(image, prev_center, window_width, margin, conv_signal):
# Find the best centroid by using past center as a reference
# Use window_width/2 as offset because convolution signal reference is at right side of
# window, not center of window
offset = window_width/2
min_index = int(max(prev_center+offset-margin,0))
max_index = int(min(prev_center+offset+margin,warped.shape[1]))
center = np.argmax(conv_signal[min_index:max_index])+min_index-offset
return center
def get_lane_pts_conv(image, window_width, window_height, margin, out_img=None):
'''Fit polynomials to lane lines using convolutional window positioning method'''
left_lane_pts = []
right_lane_pts = []
######### Compute inital values for level 0
# First find the two starting positions for the left and right lane windows by using np.sum
# to get the vertical image slice and then np.convolve the vertical image slice with the
# window template.
# Sum quarter bottom of image to get slice, could use a different ratio
l_sum = np.sum(image[int(3*image.shape[0]/4):,:int(image.shape[1]/2)], axis=0)
l_center = np.argmax(np.convolve(np.ones(window_width),l_sum))-window_width/2
r_sum = np.sum(image[int(3*image.shape[0]/4):,int(image.shape[1]/2):], axis=0)
r_center = np.argmax(np.convolve(np.ones(window_width),r_sum))-window_width/2+int(warped.shape[1]/2)
# Identify the nonzero pixels in x and y within the window
good_left_pts = get_windowed_pts(image, l_center, window_width, window_height, out_img, level=0)
good_right_pts = get_windowed_pts(image, r_center, window_width, window_height, out_img, level=0)
# Append first layer pixel inds
left_lane_pts.append(good_left_pts)
right_lane_pts.append(good_right_pts)
######### Go through each subsequent level using convolutional window positioning method
for level in range(1,(int)(image.shape[0]/window_height)):
# convolve vertically summed slice with rectangular window
conv_signal = compute_conv_signal(image, window_width, window_height, level)
# find window center locations yielding the most pixels within sliding margin
l_center = compute_window_center(image, l_center, window_width, margin, conv_signal)
r_center = compute_window_center(image, r_center, window_width, margin, conv_signal)
# Identify the nonzero pixels within the windows
good_left_pts = get_windowed_pts(image, l_center, window_width, window_height, out_img, level=level)
good_right_pts = get_windowed_pts(image, r_center, window_width, window_height, out_img, level=level)
# Append pixel pts
left_lane_pts.append(good_left_pts)
right_lane_pts.append(good_right_pts)
# Concatenate the arrays of indices
left_lane_pts = np.concatenate(left_lane_pts)
right_lane_pts = np.concatenate(right_lane_pts)
# Draw polynomial lane curves on visualization image
draw_polyfit(left_lane_pts, out_img)
draw_polyfit(right_lane_pts, out_img)
return left_lane_pts, right_lane_pts
def get_lane_pts_pixelmean(image, nwindows, margin, minpix, out_img=None):
'''Fit polynomials to lane lines using pixel mean window positioning method'''
left_lane_pts = []
right_lane_pts = []
window_height = image.shape[0]//nwindows
######### Compute inital values for left and right window positions
# Take a histogram of the bottom half of the image
histogram = np.sum(image[image.shape[0]//2:,:], axis=0)
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = histogram.shape[0]//2
leftx_current = np.argmax(histogram[:midpoint])
rightx_current = np.argmax(histogram[midpoint:]) + midpoint
######### Go through each level using pixel mean window positioning method
for level in range(nwindows):
# Identify the nonzero pixels in x and y within the window
good_left_pts = get_windowed_pts(image, leftx_current, margin*2, window_height, out_img, level=level)
good_right_pts = get_windowed_pts(image, rightx_current, margin*2, window_height, out_img, level=level)
# Append these indices to the lists
left_lane_pts.append(good_left_pts)
right_lane_pts.append(good_right_pts)
# If you found > minpix pixels, recenter next window on their mean position
if len(good_left_pts) > minpix:
leftx_current = np.int(good_left_pts[:,1].mean())
if len(good_right_pts) > minpix:
rightx_current = np.int(good_right_pts[:,1].mean())
# Concatenate the arrays of indices
left_lane_pts = np.concatenate(left_lane_pts)
right_lane_pts = np.concatenate(right_lane_pts)
# Draw polynomial lane curves on visualization image
draw_polyfit(left_lane_pts, out_img)
draw_polyfit(right_lane_pts, out_img)
return left_lane_pts, right_lane_pts
def get_polymargin_pts(image, fit_coefs, margin, out_img=None):
# Identify the x and y positions of all nonzero pixels in the image
nonzero = np.transpose(image.nonzero())
nonzeroy = nonzero[:,0]
nonzerox = nonzero[:,1]
# Identify the nonzero pixels in x and y within the window
windowed_pts = nonzero[(nonzerox > (fit_coefs[0]*(nonzeroy**2)
+ fit_coefs[1]*nonzeroy
+ fit_coefs[2]
- margin)) &
(nonzerox < (fit_coefs[0]*(nonzeroy**2)
+ fit_coefs[1]*nonzeroy
+ fit_coefs[2]
+ margin)), :]
if out_img is not None:
# Generate x and y values for plotting the search boundaries
ploty = np.arange(0,image.shape[0],10)
fitx = fit_coefs[0]*ploty**2 + fit_coefs[1]*ploty + fit_coefs[2]
# Draw the search boundaries
pts = np.stack((np.int32(np.round(fitx - margin)), np.int32(ploty)), axis=1).reshape((-1,1,2))
cv2.polylines(out_img,[pts],False,(0,255,0), 3)
pts = np.stack((np.int32(fitx + margin), np.int32(ploty)), axis=1).reshape((-1,1,2))
cv2.polylines(out_img,[pts],False,(0,255,0), 3)
return windowed_pts
def get_lane_pts_poly(image, prev_left_fit, prev_right_fit, margin, out_img=None):
'''Fit polynomials to lane lines using previous polynomial with margin pixel selection method'''
left_lane_pts = get_polymargin_pts(image, fit_coefs=prev_left_fit, margin=margin, out_img=out_img)
right_lane_pts = get_polymargin_pts(image, fit_coefs=prev_right_fit, margin=margin, out_img=out_img)
# Draw polynomial lane curves on visualization image
draw_polyfit(left_lane_pts, out_img)
draw_polyfit(right_lane_pts, out_img)
return left_lane_pts, right_lane_pts
In [10]:
plt.close('all')
examplefiles = glob.glob('test_images/*.jpg')
for examplefile in examplefiles:#['test_images/test4.jpg']:#examplefiles:
# Load a test image
img = mpimg.imread(examplefile)
# Correct for camera distortion
dst = cv2.undistort(img, mtx, dist, None, mtx)
# Get binary mask image
combined,_ = get_pixel_mask(dst)
# Warp binary mask images
warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
# Select lane line points using convolution window positioning
window_width = 100
window_height = 80 # Break image into 9 vertical layers since image height is 720
margin = 50 # How much to slide left and right for searching
warped_conv = np.dstack([warped]*3) # color image to draw on for visualization of polyfit
left_pts_conv, right_pts_conv = get_lane_pts_conv(warped, window_width, window_height, margin,
out_img=warped_conv)
# Select lane line points using pixel mean window positioning
nwindows = 9 # Choose the number of sliding windows
margin = 100 # Set the width of the windows +/- margin
minpix = 350 # Set minimum number of pixels found to recenter window
warped_pmean = np.dstack([warped]*3) # color image to draw on for visualization of polyfit
left_pts_pmean, right_pts_pmean = get_lane_pts_pixelmean(warped, nwindows, margin, minpix, out_img=warped_pmean)
# Select lane line points using existing polynomial with margin
margin = 100 # Set search width surrounding previous lane line curve
left_fit = np.polyfit(left_pts_pmean[:, 0], left_pts_pmean[:, 1], 2)
right_fit = np.polyfit(right_pts_pmean[:, 0], right_pts_pmean[:, 1], 2)
warped_poly = np.dstack([warped]*3) # color image to draw on for visualization of polyfit
left_pts_poly, right_pts_poly = get_lane_pts_poly(warped, left_fit, right_fit, margin, out_img=warped_poly)
# Plot all lane pixel fitting methods
fig, ax = plt.subplots(2, 2, figsize=(16, 10))
ax[0,0].imshow(warped_conv)
ax[0,0].set_title(examplefile + ' lane fit conv\n(not used in pipeline)', fontsize=15)
ax[0,1].imshow(warped_pmean)
ax[0,1].set_title(examplefile + ' lane fit pixelmean\n(used in pipeline on reset)', fontsize=15)
ax[1,1].imshow(warped_poly)
ax[1,1].set_title(examplefile + ' lane fit poly\n(used in pipeline nominally)', fontsize=15)
fig.delaxes(ax[1,0])
name, ext = os.path.splitext(os.path.basename(examplefile))
fig.savefig('examples/' + name + '_polyfit' + ext, bbox_inches='tight')
6. Determine the curvature of the lane and vehicle position with respect to center.¶
In [11]:
def fit_pts(lane_pts, ym_per_pix=1, xm_per_pix=1):
'''
Compute 2nd order polynomial curve fit of points with optional
conversion of pixel units to meters.
Default conversion is no conversion (1m/pixel)
Parameters
----------
lane_pts : nx2 numpy array
array of (y,x) positions of this lane line's pixels
ym_per_pix : float
meters per pixel in the y dimension
xm_per_pix : float
meters per pixel in the x dimension
Returns
-------
fit_m : float
2nd order polynomial fit coefficients in meters
'''
# Convert pixels to meters
lane_pts_m = lane_pts*np.array([[ym_per_pix, xm_per_pix]])
# Fit new polynomials to x,y in world space
fit_m = np.polyfit(lane_pts_m[:,0], lane_pts_m[:,1], 2)
return fit_m
def compute_curvature(fit, yeval):
'''
Compute radius of curvature of a 2nd order polynomial at yeval.
Parameters
----------
yeval : float
y value at which to compute radius of curvature.
Returns
-------
curverad : float
radius of curvature at yeval
'''
# Calculate radius of curvature at yeval
curverad = ((1 + (2*fit[0]*yeval + fit[1])**2)**1.5) / np.abs(2*fit[0])
return curverad
In [12]:
plt.close('all')
examplefiles = glob.glob('test_images/*.jpg')
for examplefile in examplefiles:#['test_images/test4.jpg']:#examplefiles:
# Load a test image
img = mpimg.imread(examplefile)
# Correct for camera distortion
dst = cv2.undistort(img, mtx, dist, None, mtx)
# Get binary mask image
combined,_ = get_pixel_mask(dst)
# Warp binary mask images
warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
# Select lane line points using pixel mean window positioning
nwindows = 9 # Choose the number of sliding windows
margin = 100 # Set the width of the windows +/- margin
minpix = 350 # Set minimum number of pixels found to recenter window
warped_pmean = np.dstack([warped]*3) # color image to draw on for visualization of outcome
left_pts, right_pts = get_lane_pts_pixelmean(warped, nwindows, margin, minpix, out_img=warped_pmean)
# Compute polynomial fit in meters
ym_per_pix = 30/720
xm_per_pix = 3.7/700
left_fit_m = fit_pts(left_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
right_fit_m = fit_pts(right_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
# Compute radii of curvature for each lane line
yeval_m = (warped.shape[0] - 1) * ym_per_pix # evaluate at bottom of image (car position)
left_curverad = compute_curvature(left_fit_m, yeval_m)
right_curverad = compute_curvature(right_fit_m, yeval_m)
nleft = len(left_pts)
nright = len(right_pts)
curverad = (left_curverad if nleft > nright else right_curverad)
# Compute car's offset from center of lane in meters
left_m, right_m = np.polyval(left_fit_m, yeval_m), np.polyval(right_fit_m, yeval_m)
lane_pos_m = (left_m + right_m) / 2
car_pos_m = (warped.shape[1] * xm_per_pix) / 2
car_offset = car_pos_m - lane_pos_m # negative means car is left of center
# Plot lane line fit and radius of curvature
plt.figure(figsize=(8,5))
plt.imshow(warped_pmean)
titlestr = ('{} lane fit\n left radius = {}m, right radius = {}m\n best radius = {}m, offset = {:.2f}m'
.format(examplefile, np.int(left_curverad), np.int(right_curverad), np.int(curverad),
car_offset))
plt.title(titlestr, fontsize=15)
name, ext = os.path.splitext(os.path.basename(examplefile))
plt.gcf().savefig('examples/' + name + '_curverad' + ext, bbox_inches='tight')
In [13]:
def draw_lane_region(left_fit, right_fit, out_img):
'''Draw lane region onto out_img'''
# Generate x and y values for plotting the polynomials
ploty = np.arange(0, out_img.shape[0], 1)
left_fitx = np.polyval(left_fit, ploty)
right_fitx = np.polyval(right_fit, ploty)
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(out_img, np.int_([pts]), (0,255, 0))
In [14]:
plt.close('all')
examplefiles = glob.glob('test_images/*.jpg')
for examplefile in examplefiles:
# Load a test image
img = mpimg.imread(examplefile)
# Correct for camera distortion
dst = cv2.undistort(img, mtx, dist, None, mtx)
# Get binary mask image
combined,_ = get_pixel_mask(dst)
# Warp binary mask images
warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
# Select lane line points using pixel mean window positioning
nwindows = 9 # Choose the number of sliding windows
margin = 100 # Set the width of the windows +/- margin
minpix = 350 # Set minimum number of pixels found to recenter window
warped_pmean = np.dstack([warped]*3) # color image to draw on for visualization of outcome
left_pts, right_pts = get_lane_pts_pixelmean(warped, nwindows, margin, minpix, out_img=warped_pmean)
# Compute polynomial fit in meters
ym_per_pix = 30/720
xm_per_pix = 3.7/700
left_fit_m = fit_pts(left_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
right_fit_m = fit_pts(right_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
# Compute radii of curvature for each lane line
yeval_m = (warped.shape[0] - 1) * ym_per_pix # evaluate at bottom of image (car position)
left_curverad = compute_curvature(left_fit_m, yeval_m)
right_curverad = compute_curvature(right_fit_m, yeval_m)
nleft = len(left_pts)
nright = len(right_pts)
curverad = (left_curverad if nleft > nright else right_curverad)
# Compute car's offset from center of lane in meters
left_m, right_m = np.polyval(left_fit_m, yeval_m), np.polyval(right_fit_m, yeval_m)
lane_pos_m = (left_m + right_m) / 2
car_pos_m = (warped.shape[1] * xm_per_pix) / 2
car_offset = car_pos_m - lane_pos_m # negative means car is left of center
# Compute polynomial fit in pixels
left_fit = np.polyfit(left_pts[:, 0], left_pts[:, 1], 2)
right_fit = np.polyfit(right_pts[:, 0], right_pts[:, 1], 2)
# Draw lane region on unwarped color image
lane_img = np.dstack([np.zeros_like(warped, np.uint8)]*3) # blank color image for drawing
draw_lane_region(left_fit, right_fit, lane_img)
lane_img_unwarped = cv2.warpPerspective(lane_img, Minv, imgsize) # unwarp top-down lane image
lane_overlay = cv2.addWeighted(dst, 1, lane_img_unwarped, 0.3, 0) # mix with car-perpective image
# Draw radius of curvature and lane offset text onto lane overlay
imgtext = 'Radius of Curvature = {}m'.format(np.int(curverad))
cv2.putText(lane_overlay, imgtext, (150,100), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 7)
side = ('left' if car_offset < 0 else 'right')
imgtext = 'Vehicle is {:.2f}m {} of center'.format(np.abs(car_offset), side)
cv2.putText(lane_overlay, imgtext, (150,200), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 7)
# Plot lane line fit and radius of curvature
fig, ax = plt.subplots(1, 2, figsize=(16, 5))
ax[0].imshow(lane_img)
ax[0].set_title(examplefile + ' lane region top-down', fontsize=15)
ax[1].imshow(lane_overlay)
ax[1].set_title(examplefile + ' lane region overlay', fontsize=15)
name, ext = os.path.splitext(os.path.basename(examplefile))
plt.gcf().savefig('examples/' + name + '_regionoverlay' + ext, bbox_inches='tight')
Define pipeline function and Run on video¶
In [15]:
from collections import deque
# Define a class to receive the characteristics of each line detection
class Line():
def __init__(self):
# was the line detected in the last iteration?
self.detected = False
#polynomial coefficient exponential moving average
self.avg_fit = None
#polynomial coefficients for the most recent fits
self.current_fit = [np.array([False])]
#radius of curvature of the line in some units
self.radius_of_curvature = None
#distance in meters of vehicle center from the line
self.line_base_pos = None
#difference in fit coefficients between last and new fits
self.diffs = np.array([0,0,0], dtype='float')
#(y,x) pairs for all detected line pixels
self.allpts = None
leftline = Line()
rightline = Line()
In [16]:
def process_image(img):
global leftline
global rightline
# Correct for camera distortion
dst = cv2.undistort(img, mtx, dist, None, mtx)
# Get binary mask image
combined,_ = get_pixel_mask(dst)
# Warp binary mask image
warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
if leftline.detected and rightline.detected:
# Select lane line points using existing polynomial with margin
left_fit = leftline.current_fit
right_fit = rightline.current_fit
margin = 100 # Set search width surrounding previous lane line curve
left_pts, right_pts = get_lane_pts_poly(warped, left_fit, right_fit, margin)
else:
# Select lane line points using pixel mean window positioning
nwindows = 9 # Choose the number of sliding windows
margin = 100 # Set the width of the windows +/- margin
minpix = 350 # Set minimum number of pixels found to recenter window
left_pts, right_pts = get_lane_pts_pixelmean(warped, nwindows, margin, minpix)
# Compute polynomial fit in meters
ym_per_pix = 30/720
xm_per_pix = 3.7/700
left_fit_m = fit_pts(left_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
right_fit_m = fit_pts(right_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
# Compute radii of curvature for each lane line
yeval_m = (warped.shape[0] - 1) * ym_per_pix # evaluate at bottom of image (car position)
left_curverad = compute_curvature(left_fit_m, yeval_m)
right_curverad = compute_curvature(right_fit_m, yeval_m)
nleft = len(left_pts)
nright = len(right_pts)
# Compute car's offset from center of lane in meters
left_m, right_m = np.polyval(left_fit_m, yeval_m), np.polyval(right_fit_m, yeval_m)
lane_pos_m = (left_m + right_m) / 2
car_pos_m = (warped.shape[1] * xm_per_pix) / 2
car_offset = car_pos_m - lane_pos_m # negative means car is left of center
# Compute polynomial fit in pixels
left_fit = np.polyfit(left_pts[:, 0], left_pts[:, 1], 2)
right_fit = np.polyfit(right_pts[:, 0], right_pts[:, 1], 2)
# Sanity Check
curve_diff = left_curverad - right_curverad
curve_diff_ok = np.abs(curve_diff) < 500
curverad_debug = (left_curverad if nleft > nright else right_curverad)
# print('curve_diff', curve_diff)
# print('curverad', curverad_debug)
curve_ok = curve_diff_ok or (curverad > 3000)
y = np.linspace(0, yeval_m)
separations = np.polyval(right_fit_m, y) - np.polyval(left_fit_m, y)
separation_ok = np.abs(separations.mean() - 3.7) < 0.5
# print('separations.mean()', separations.mean())
parallel_ok = separations.std() < 0.06
# print('separations.std()', separations.std())
pixel_count_ok = (nleft > 5000) and (nright > 5000)
# print('nleft', nleft, 'nright', nright)
# For now, only using lane line separation check
if separation_ok:
# store detected characteristics for use on future sanity failures
leftline.detected = True
rightline.detected = True
leftline.allpts = left_pts
rightline.allpts = right_pts
leftline.radius_of_curvature = left_curverad
rightline.radius_of_curvature = right_curverad
leftline.line_base_pos = left_m
rightline.line_base_pos = right_m
leftline.current_fit = left_fit
rightline.current_fit = right_fit
if leftline.avg_fit is None:
leftline.avg_fit = left_fit
rightline.avg_fit = right_fit
else:
leftline.avg_fit = left_fit*0.1 + leftline.avg_fit*0.9 # exponential moving average filter
rightline.avg_fit = right_fit*0.1 + rightline.avg_fit*0.9 # exponential moving average filter
left_fit = leftline.avg_fit
right_fit = rightline.avg_fit
else:
# sanity failed, load last known good values
leftline.detected = False
leftline.detected = False
if leftline.radius_of_curvature is not None:
left_fit = leftline.avg_fit
right_fit = rightline.avg_fit
left_curverad = leftline.radius_of_curvature
right_curverad = rightline.radius_of_curvature
left_m = leftline.line_base_pos
right_m = rightline.line_base_pos
lane_pos_m = (left_m + right_m) / 2
car_pos_m = (warped.shape[1] * xm_per_pix) / 2
car_offset = car_pos_m - lane_pos_m # negative means car is left of center
# Draw lane region on unwarped color image
lane_img = np.dstack([np.zeros_like(warped, np.uint8)]*3) # blank color image for drawing
draw_lane_region(left_fit, right_fit, lane_img)
lane_img_unwarped = cv2.warpPerspective(lane_img, Minv, imgsize) # unwarp top-down lane image
lane_overlay = cv2.addWeighted(dst, 1, lane_img_unwarped, 0.3, 0) # mix with car-perpective image
# Draw radius of curvature and lane offset text onto lane overlay
best_curverad = (left_curverad if nleft > nright else right_curverad)
imgtext = 'Radius of Curvature = {}m'.format(np.int(best_curverad))
cv2.putText(lane_overlay, imgtext, (150,100), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 7)
side = ('left ' if car_offset < 0 else 'right')
imgtext = 'Vehicle is {:.2f}m {} of center'.format(np.abs(car_offset), side)
cv2.putText(lane_overlay, imgtext, (150,200), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 7)
# Debug text
# imgtext = 'curve_diff {:.0f}, curverad {:.0f}, sepmean {:.2f}, sepstd {:.4f}'.format(curve_diff,
# curverad_debug,
# separations.mean(),
# separations.std())
# cv2.putText(lane_overlay, imgtext, (0,30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,255,255), 3)
return lane_overlay
In [17]:
from moviepy.editor import VideoFileClip
video_output = 'project_video_out.mp4'
clip1 = VideoFileClip("project_video.mp4")
white_clip = clip1.fl_image(process_image) #NOTE: this function expects color images!!
%time white_clip.write_videofile(video_output, audio=False)
In [18]:
from IPython.display import HTML
HTML("""
<video width="960" height="540" controls>
<source src="{0}">
</video>
""".format(video_output))
Out[18]:
In [ ]: